#include <iostream>
#include <cmath>
#include <memory.h>
#include "ros/ros.h"
#include "jsk_recognition_msgs/BoundingBoxArray.h"
#include "std_msgs/Float64MultiArray.h"
#include "geometry_msgs/PoseStamped.h"
#define debug 0
struct point
{
    double x, y;
    point operator+(const point& p)const throw(){return {x + p.x, y + p.y};}
    point operator-(const point& p)const throw(){return {x - p.x, y - p.y};}
    point operator*(double number)const throw(){return {number * x, number * y};}
    point operator/(double number)const throw(){return {x / number, y / number};}
    double distance(const point& p2)const throw(){return std::sqrt(std::pow(x - p2.x, 2) + std::pow(y - p2.y, 2));}
    point& operator+=(const point& p)throw(){x += p.x; y += p.y; return *this;}
    point& operator*=(double number)throw(){x *= number; y *= number; return *this;}
    point& operator/=(double number)throw(){x /= number; y /= number; return *this;}
    bool not00()const throw(){return x or y;}
    bool is00()const throw(){return not not00();}
};
struct control
{
    double v; double delta;
}control_actual;
struct state
{
    double x = 0; double y = 0; double yaw;
    void update(const control& c = control_actual, double dt = 0.01)throw()
    {
        x += c.v * std::cos(c.delta) *dt;
        y += c.v * std::sin(c.delta) * dt;
        yaw += c.v * std::tan(c.delta) * dt;
    }
}state_actual;
constexpr int expected_count = 10;
point center_points[expected_count][16];//0到75线有16对桶
void lidar_callback(const jsk_recognition_msgs::BoundingBoxArray& lidar)throw()
{
    static int valid_count = 1;//有效回调（指存在有效数据的回调）次数
    //循环队列的起点为valid_count <= expected_count ? 0 : valid_count % expected_count;
    //本次填写位置(valid_count  - 1) % expected_count
    static int count[16];//每对桶出现的次数，计算平均用
    std::pair<point, point> mate[16];
    memset(mate, 0, sizeof(mate));
    for (const auto& p : lidar.boxes)
    {
#if debug
        std::cout << p.pose.position.x << '\t' << p.pose.position.y << '\n';
#endif
        if (p.pose.position.x > -0.5 and p.pose.position.x < 80)// and std::abs(p.pose.position.y) < 5.0)
        {
            bool not_found = true;
            point* this_time = center_points[valid_count <= expected_count ? 0 : valid_count % expected_count];
            double i = (p.pose.position.x - 0.3) / 5.0;
            int index = std::round(i);
            if (std::abs(i - index) < 0.5)//0.2
            {
                if (mate[index].second.not00())
                {
                    if (std::abs(mate[index].second.x - p.pose.position.x) < 0.5 and std::abs(mate[index].second.y - p.pose.position.y) < 0.5)
                    {
                        mate[index].second.x = (mate[index].second.x + p.pose.position.x) / 2.0;
                        mate[index].second.y = (mate[index].second.y + p.pose.position.y) / 2.0;
                    }
                    else if (std::abs(mate[index].first.x - p.pose.position.x) < 0.5 and std::abs(mate[index].first.y - p.pose.position.y) < 0.5)
                    {
                        mate[index].first.x = (mate[index].first.x + p.pose.position.x) / 2.0;
                        mate[index].first.y = (mate[index].first.y + p.pose.position.y) / 2.0;
                    }
                    else
                    {
                        const double abs_p_y = std::abs(p.pose.position.y);
                        if (abs_p_y < std::abs(mate[index].second.y))
                            if (abs_p_y < std::abs(mate[index].first.y))
                            {
                                mate[index].second = mate[index].first;
                                mate[index].first = {p.pose.position.x, p.pose.position.y};
                            }
                            else
                            mate[index].second = {p.pose.position.x, p.pose.position.y};
                    }
                }
                else if (mate[index].first.not00())
                {
                    if (std::abs(mate[index].first.x - p.pose.position.x) < 0.5 and std::abs(mate[index].first.y - p.pose.position.y) < 0.5)
                    {
                        mate[index].first.x = (mate[index].first.x + p.pose.position.x) / 2.0;
                        mate[index].first.y = (mate[index].first.y + p.pose.position.y) / 2.0;
                    }
                    else if (std::abs(p.pose.position.y) < std::abs(mate[index].first.y))
                    {
                        mate[index].second = mate[index].first;
                        mate[index].first = {p.pose.position.x, p.pose.position.y};
                    }
                    else
                        mate[index].second = {p.pose.position.x, p.pose.position.y};
                }
                else
                    mate[index].first = {p.pose.position.x, p.pose.position.y};
            }
        }
    }
    int this_time = (valid_count  - 1) % expected_count;
    bool valid = false;
    for (int i = 0;i < 16;++i)
        if (mate[i].first.not00() and mate[i].second.not00())
        {
            valid = true;
            center_points[this_time][i] = (mate[i].first + mate[i].second) / 2.0;
        }
    valid_count += valid;
}
double yaw0()throw()
{
    point average_center_points[16];
    for (int i = 0;i < 16;++i)
    {
        int count = 0;
        for (int times = 0;times < expected_count;++times)
            if (center_points[times][i].not00())
            {
                average_center_points[i] += center_points[times][i];
                ++count;
            }
        average_center_points[i] /= count;
    }
    double distance_point[16][16] = {{0.0}};
    for (int i = 0;i < 16;++i)
        for (int j = 0;j < i;++j)
            if (average_center_points[i].not00())
                distance_point[i][j] = average_center_points[i].distance(average_center_points[j]);
    // double distance[16][16][16] = {{{0}}};
    int votes[16] = {0};
    int sum = 0;
    for (int i = 0;i < 16;++i)
    {
        if (average_center_points[i].not00())
            for (int j = 0;j < i;++j)
                if (average_center_points[i].not00())
                    for (int k = 0;k < j;++k)
                        if (average_center_points[i].not00())
                        {
                            sum += 3;
                            const double p = (distance_point[i][j] + distance_point[i][k] + distance_point[j][k]) / 2.0;
                            const double double_area = 2.0 * std::sqrt(p * (p - distance_point[i][j]) * (p - distance_point[i][k]) * (p - distance_point[j][k]));
                            votes[i] += double_area / distance_point[j][k] > 0.5;
                            votes[j] += double_area / distance_point[i][k] > 0.5;
                            votes[k] += double_area / distance_point[i][j] > 0.5;
                            // distance[i][j][k] = double_area / distance_point[i][j];//k到ij
                        }
    }
    int n = 0;
    double sum_xy = 0.0;
    double sum_xx = 0.0;
    double sum_x = 0.0;
    double sum_y = 0.0;
    for (int i = 0;i < 16;++i)
    {
        if (votes[i] / (double)sum < 0.15)
        {
            ++n;
            sum_xy += average_center_points[i].x * average_center_points[i].y;
            sum_xx += average_center_points[i].x * average_center_points[i].x;
            sum_x += average_center_points[i].x;
            sum_y += average_center_points[i].y;
            // valid_points.push_back(average_center_points[i]);
        }
    }
    return -std::atan2((n * sum_xy - sum_x * sum_y), (n * sum_xx - sum_x * sum_x));
}
bool not_ready = true;//初始未收到go
void go_callback(const std_msgs::Float64MultiArray& ecu_go)throw()
{//ecu_go.data[2]为0时表示还没有go，为1表示go
    not_ready = ecu_go.data[2] < 0.5;//防止精度原因误判
}
void delta_callback(const std_msgs::Float64MultiArray& ecu_msg)throw()
{
    control_actual.delta = ecu_msg.data[0];
}
void speed_callback(const geometry_msgs::PoseStamped& pose)throw()
{
    control_actual.v = pose.pose.orientation.w;
    state_actual.update();
}
int main(int argc, char** argv)throw()
{
    ros::init(argc, argv, "line_path");
    ros::NodeHandle n;
    ros::Rate loop_rate(10);
    ros::Publisher command(n.advertise<std_msgs::Float64MultiArray>("/command", 1));
    std_msgs::Float64MultiArray msg;
    msg.data.resize(5);
    msg.data[2] = 1;//不紧急制动
    msg.data[3] = 0;//制动等级，目前没用
    msg.data[4] = 2;//行驶
    msg.data[0] = msg.data[1] = 0;
    for(int i = 0;i < 20;++i)
    {
        command.publish(msg);
        loop_rate.sleep();
    }
    {//出发前的工作
        ros::Subscriber go(n.subscribe("/ecu_msg", 1, go_callback));//检测go信号
        ros::Subscriber lidar(n.subscribe("/detected_bounding_box", 1, lidar_callback));//雷达消息
        //便于用Ctrl + C终止程序；未收到go信号则继续检测
        while (ros::ok() and not_ready)
            ros::spinOnce();
    }//至此不再接收go和雷达消息，两个subscriber自动销毁
    // point target = last_point();
    state_actual.yaw = yaw0();
    ros::Subscriber delta_actual(n.subscribe("/ecu_msg", 1, delta_callback));//ecu前轮转角
    ros::Subscriber speed_actual(n.subscribe("/current_pose", 1, speed_callback));//惯导速率
    while (state_actual.x < 80)
    {
        msg.data[0] = std::min(1.5, control_actual.v + 0.2);
        msg.data[1] = std::min(15, std::atan(15.11 * state_actual.yaw / control_actual.v));
        command.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }
    msg.data[0] = msg.data[1] = 0;
    while (state_actual.x < 95)
    {
        command.publish(msg);
        loop_rate.sleep();
    }
    msg.data[4] = 3;
    command.publish(msg);
}